
/**********************************************************************\
*																																	    *
* PROGRAM: Sensor Reading Programm For Get Over It 2010-2011    	    *
* VERSION: 1.0  																									    *
* PURPOSE: Controllable task to read certain sensors. This task in    *
*          necessary because there is no thread protection within     *
*          RobotC, which causes procedures to be read twice at once   *
* AUTHOR: Benjamin Chaney                                             *
* DATE:		 April 2011  	  																					  *
*	In Partnership with Xander Soldat																																	  *
* LICENSE: GNU GPL V3 2011                                            *
\**********************************************************************/

#ifndef SENSORREAD
#define SENSORREAD
#endif

bool readEOPDL = false;       /* <!Set true to read Left EOPD sensors> */
bool readEOPDR = false;       /* <!Set true to read Right EOPD sensors> */
bool readKalman = true;     /* <!Set true to read Compass and gyro sensors> */
bool readAccel = false;      /* <!Set true to read Accel sensors> */
bool initCliff = false;      /* <!Set true to initialize the IR distance sensors' low-pass filter> */
bool readCliff = false;    /* <!Set true to read IR distance sensors> */
bool readPot = false;      /* <!Set true to read Arm Potentiometer sensors> */
bool gyrocal = false;
bool accelcal = false;

/*
* These are the outputs from the sensor task.
*/

/*
*
*/
bool AccelRead;
bool EOPDReadL;
bool EOPDReadR;
bool KalmanRead;
bool IRread;
bool PotRead;
bool cliffReset;

int AccelReadingX();
int AccelReadingY();
int AccelReadingZ();
int EOPDLReading();
int EOPDRReading();
int CompassReading();
int GyroReading();
int PotReading();
void initIRLowPass();
int updateFallingMap(); // update the "falling" map based on IR distance sensor readings

int _AccelcalX;
int _AccelcalY;
int _AccelcalZ;
int _AccelReadingX;
int _AccelReadingY;
int _AccelReadingZ;
int _EOPDLReading;
int _EOPDRReading;
int _CompassReading;
int _GyroReading;
int _PotReading;
int _IRA;
int _IRB;
int _IRC;
int _IRD;


task sensorRead() {
  while(true) {
    if (readEOPDL) {
      _EOPDLReading = HTEOPDreadRaw(EOPD_L);
      EOPDReadL = true;
    }
    if (readEOPDR) {
      _EOPDRReading = HTEOPDreadRaw(EOPD_R);
      EOPDReadR = true;
    }
    if (readKalman) {
      _CompassReading = HTMCreadHeading(COMPASSPOS);
      _GyroReading = HTGYROreadRot(GYROPOS);
      KalmanRead = true;
    }
    if (readAccel) {
      HTACreadAllAxes(S3, _AccelReadingX, _AccelReadingY, _AccelReadingZ);
      _AccelReadingX -= _AccelcalX;
      _AccelReadingY -= _AccelcalY;
      _AccelReadingZ -= _AccelcalZ;
      AccelRead = true;
    }
    if (initCliff) {
      _IRA = HTPBreadADC(HTPB, WHEEL_IR_FR, 10);
      _IRB = HTPBreadADC(HTPB, WHEEL_IR_BR, 10);
      _IRC = HTPBreadADC(HTPB, WHEEL_IR_BL, 10);
      _IRD = HTPBreadADC(HTPB, WHEEL_IR_FL, 10);
      cliffReset = true;
    }
    if (readCliff) {
      _IRA = _IRA * (1-LOW_PASS_FACTOR) + LOW_PASS_FACTOR * HTPBreadADC(HTPB, WHEEL_IR_FR, 10);
      _IRB = _IRB * (1-LOW_PASS_FACTOR) + LOW_PASS_FACTOR * HTPBreadADC(HTPB, WHEEL_IR_BR, 10);
      _IRC = _IRC * (1-LOW_PASS_FACTOR) + LOW_PASS_FACTOR * HTPBreadADC(HTPB, WHEEL_IR_BL, 10);
      _IRD = _IRD * (1-LOW_PASS_FACTOR) + LOW_PASS_FACTOR * HTPBreadADC(HTPB, WHEEL_IR_FL, 10);
      IRread = true;
    }
    if (readPot) {
      _PotReading =  HTPBreadADC(HTPB, POTPOS, 10);
      PotRead = true;
    }
    if (gyrocal){
      HTGYROstartCal(GYROPOS);
      gyrocal = false;
    }
    if (accelcal){
      HTACreadAllAxes(S3, _AccelcalX, _AccelcalY, _AccelcalZ);
      accelcal = false;
    }
    EndTimeSlice();
  }
}

int AccelReadingX() {
  AccelRead = false;
  readAccel = true;
  while (!AccelRead) {
    readAccel = true;
    EndTimeSlice();
  }
  return _AccelReadingX;
}

int AccelReadingY() {
  AccelRead = false;
  readAccel = true;
  while (!AccelRead) {
    readAccel = true;
    EndTimeSlice();
  }
  readAccel = false;
  return _AccelReadingY;
}

int AccelReadingZ() {
  AccelRead = false;
  readAccel = true;
  while (!AccelRead) {
    readAccel = true;
    EndTimeSlice();
  }
  readAccel = false;
  return _AccelReadingZ;
}

int EOPDRReading() {
  EOPDReadR = false;
  readEOPDR = true;
  while (!EOPDReadR) {
    readEOPDR = true;
    EndTimeSlice();
  }
  readEOPDR = false;
  return _EOPDRReading;
}

int EOPDLReading() {
  EOPDReadL = false;
  readEOPDL = true;
  while (!EOPDReadL) {
    readEOPDL = true;
    EndTimeSlice();
  }
  readEOPDL = false;
  return _EOPDLReading;
}
int CompassReading() {
  KalmanRead = false;
  readKalman = true;
  while (!KalmanRead) {
    readKalman = true;
    EndTimeSlice();
  }
  readKalman = false;
  return _CompassReading;
}

int GyroReading() {
  KalmanRead = false;
  readKalman = true;
  while (!KalmanRead) {
    readKalman = true;
    EndTimeSlice();
  }
  readKalman = false;
  return _GyroReading;
}

int PotReading() {
  PotRead = false;
  readPot = true;
  while (!PotRead) {
    readPot = true;
    EndTimeSlice();
  }
  readPot = false;
  return _PotReading;
}

/*********************** Start IR distance sensors mgmt ****************************/


void readIR() {
  IRread = false;
  readCliff = true;
  while (!IRread){
    readCliff = true;
    EndTimeSlice();
  }
  readCliff = false;
}

int updateFallingMap() {
  readIR();
  int fallingMap = 0;
  if (_IRD < EDGE_FL || _IRD > HIGH_EDGE_FL) {      // IR Forward Left is off of bridge
      fallingMap += IR_FL;
  }
  if (_IRA < EDGE_FR || _IRA > HIGH_EDGE_FR) {      // IR Forward Right is off bridge
      fallingMap += IR_FR;
  }
  if (_IRC < EDGE_BL || _IRC > HIGH_EDGE_BL) {      // IR Back Left is off bridge
      fallingMap += IR_BL;
  }
  if (_IRB < EDGE_BR || _IRB > HIGH_EDGE_BR) {      // IR Back Right is off bridge
      fallingMap += IR_BR;
  }
  return fallingMap;
}

void cliffResetFunc() {
  cliffReset = false;
  initCliff = true;
  while (!cliffReset){
    initCliff = true;
    EndTimeSlice();
  }
  initCliff = false;
}

void initIRLowPass() {
  cliffResetFunc();
  cliffReset = false;
}


/*********************** END IR distance sensors mgmt ****************************/


void calibrategyro() {
  gyrocal = true;
  while (gyrocal);
}
void calibrateAccel(){
  accelcal = true;
  while (accelcal);
}
